package com.ludomancy.sora;

import java.util.Iterator;
import java.util.Vector;

public class Game
{
	public static Game game;
	
	public double px;
	public double py;
	public double old_px;
	public double old_py;
	public double targetX;
	public double targetY;
	public double radii = 0.008;
	
	// physics
	public static final double GRAVITY = 0.000001;
	public static final double DELTA = 1;
	public static final double AIR_FRICTION = 0.0005;
	public static final int VERLET_ITERATIONS = 5;
	public static final double SPEED = 0.0015;
	public static final int PHYSICS_ITERATIONS = 18;
	public static final double PUSH_FORCE = 0.0001;
	public static final double ARROW_WIDTH = 0.0035;
	public static final double PLAYER_TO_ARROW_FRICTION = 50;
	
	// arrows
	public static final double ARROW_LENGTH = 0.15;

	public Vector vertices = new Vector();
	public Vector edges = new Vector();
	
	public Game()
	{
		game = this;
		
		// initial position in the center
		px = targetX = old_px = 0.5;
		py = targetY = old_py = 0.5;
		createArrow(0.2,0.2,0.0001,0.0002);
		createArrow(0.6,0.6,-0.0001,0.0002);
		createArrow(0.2,0.6,0.0001,-0.0002);
		createArrow(0.6,0.2,-0.0001,-0.0002);
	}
	
	public void removeEdge(Edge e)
	{
		edges.remove(e);
		vertices.remove(e.v1);
		vertices.remove(e.v2);
	}
	
	public void setTarget(double x, double y)
	{
		targetX = x;
		targetY = y;
	}
	
	private double t;
	private double contactX;
	private double contactY;
	public double distanceFromPointToEdge(double px, double py, Edge edge)
	{
		// find t for x
		double p0X = edge.v1.px;
		double p0Y = edge.v1.py;
		double p1X = edge.v2.px;
		double p1Y = edge.v2.py;
		
		double upper_part = (-( p1Y - p0Y ) * ( p0Y - py ) - ( p1X - p0X ) * ( p0X - px ));
		double denom = ( ( p1X - p0X ) * ( p1X - p0X ) + ( p1Y - p0Y ) * ( p1Y - p0Y ) );
		t =  upper_part / denom;
		if( t < 0 ) 
		{
			t = 0;
		}
		else if( t > 1 )
		{
			t = 1;
		}
		contactX = t * (p1X - p0X) + p0X;
		contactY = t * (p1Y - p0Y) + p0Y;
		return dist(contactX, contactY, px, py);
	}
	
	public static double dist(double x0, double y0, double x1, double y1)
	{
		double a = x0 - x1;
		double b = y0 - y1;
		return Math.sqrt(a*a + b*b);
	}
	
	public void createArrow(double tipX, double tipY, double forceX, double forceY)
	{
		double dirX = forceX;
		double dirY = forceY;
		
		Vertex v1 = new Vertex(tipX, tipY, forceX, forceY);
		
		double m = dirY/dirX;
		double bterm = (m*m)/(1+m*m);
		double b = (ARROW_LENGTH*ARROW_LENGTH)*(1 - bterm);
		b = Math.sqrt(b);
		double a = m*b;
		Vertex v2 = new Vertex(b + tipX, a + tipY, forceX, forceY);
		vertices.add(v1);
		vertices.add(v2);
		edges.add(new Edge(v1,v2));
	}
	
	public void update()
	{
		for (int i = 0; i < PHYSICS_ITERATIONS; i++)
		{
			updatePlayer();
			for (Iterator iter = vertices.iterator(); iter.hasNext();)
			{
				Vertex v = (Vertex) iter.next();

				// reset forces
				v.forceX = 0;
				v.forceY = GRAVITY;
				
				// put a limit on the velocity of the point
				double speed = dist(v.px, v.py, v.old_px, v.old_py);
				double LIMIT = 0.001;
				if(speed > LIMIT)
				{
					v.old_px += v.velX() * (LIMIT/speed);
					v.old_py += v.velY() * (LIMIT/speed);
				}
				
				// integrate
				double oldX = v.px;
				double oldY = v.py;
				
				v.px = (2-AIR_FRICTION) * v.px - v.old_px * (1-AIR_FRICTION) + v.forceX * (DELTA*DELTA);
				v.py = (2-AIR_FRICTION) * v.py - v.old_py * (1-AIR_FRICTION) + v.forceY * (DELTA*DELTA);
				
				v.old_px = oldX;
				v.old_py = oldY;
			}
			for (int j = 0; j < edges.size(); j++)
			{
				Edge e = (Edge) edges.elementAt(j);
				// collision against player
				computePlayerCollision(e);
			}
			enforceConstrains();
		}
	}
	
	private void updatePlayer()
	{
		old_px = px;
		old_py = py;
		// find dir
		double dirX = targetX - px;
		double dirY = targetY - py;
		if(dist(targetX, targetY, px, py) < SPEED)
		{
			px = targetX;
			py = targetY;
		}
		else
		{
			double[] norm = normalize(dirX, dirY);
			dirX = norm[0];
			dirY = norm[1];
			dirX *= SPEED;
			dirY *= SPEED;
			px += dirX;
			py += dirY;
		}
	}
	
	private void computeVertexEdgeCollision(Vertex v, double vradii, Edge e)
	{
		double d = distanceFromPointToEdge(v.px, v.py, e) - vradii/2;
		if(d < vradii && d > 0)
		{
			double distance = vradii - d;
			// relative velocity
			double velX = v.px - v.old_px;
			double velY = v.py - v.old_py;
			double vel2X = e.getVelocityAtPoint(t)[0];
			double vel2Y = e.getVelocityAtPoint(t)[1];
			double relX = velX - vel2X;
			double relY = velY - vel2Y;
			
			// impact normal
			double normX = contactX - v.px;
			double normY = contactY - v.py;
			double[] normalized = normalize(normX, normY);
			normX = normalized[0];
			normY = normalized[1];
			
			// project both out of the collision
			v.move(-normX*distance*0.5, -normY*distance*0.5);
			e.moveAtPoint(t, normX*distance*0.5, normY*distance*0.5);
			
			// compute resulting speed pushes for both
			double j = -(relX * normX + relY * normY)/(normX*normX + normY*normY);
			v.px += j*normX;
			v.py += j*normY;
			e.pushAtPoint(t, -j*normX, -j*normY);
			
			// friction vector
			double frictionX = normY;
			double frictionY = -normX;
			double relVelX = e.getVelocityAtPoint(t)[0];
			double relVelY = e.getVelocityAtPoint(t)[1];
			double[] tangential = project(relVelX, relVelY, frictionX, frictionY);
			tangential[0] *= distance*PLAYER_TO_ARROW_FRICTION;
			tangential[1] *= distance*PLAYER_TO_ARROW_FRICTION;

			e.pushAtPoint(t, -tangential[0], -tangential[1]);

		}
	}
	
	private void computePlayerCollision(Edge e)
	{
		double d = distanceFromPointToEdge(px, py, e) - ARROW_WIDTH/2;
		if(d < radii && d > 0)
		{
			double distance = radii - d;
			double pushX = px - old_px;
			double pushY = py - old_py;
			
			// impact normal
			double normX = contactX - px;
			double normY = contactY - py;
			double[] normalized = normalize(normX, normY);
			normX = normalized[0];
			normY = normalized[1];
						
			// if player is moving
			if(pushX != 0 || pushY != 0)
			{
				// calculate player velocity in the normal
				double[] playerVel = normalize(pushX, pushY);
				playerVel[0] *= SPEED * 0.01;
				playerVel[1] *= SPEED * 0.01;
				double[] projPlayerVel = project(playerVel[0], playerVel[1], normX, normY);				
				// now apply a small, controlled push
				e.pushAtPoint(t, projPlayerVel[0], projPlayerVel[1]);
				
				// teleport arrow so it does not collide
				e.moveAtPoint(t, normX*distance, normY*distance);
			}
			else
			{
				// player is static, do a normal verlet bounce
				e.pushAtPoint(t, 2*normX*distance, 2*normY*distance);
			}
			
			// friction vector
			double frictionX = normY;
			double frictionY = -normX;
			double relVelX = e.getVelocityAtPoint(t)[0];
			double relVelY = e.getVelocityAtPoint(t)[1];
			double[] tangential = project(relVelX, relVelY, frictionX, frictionY);
			tangential[0] *= distance*PLAYER_TO_ARROW_FRICTION;
			tangential[1] *= distance*PLAYER_TO_ARROW_FRICTION;

			e.pushAtPoint(t, -tangential[0], -tangential[1]);
		}
	}
	
	public static void log(String msg, double x, double y)
	{
		System.out.println(msg+" ("+x+","+y+")");
	}
	
	public static double[] project(double v1x, double v1y, double v2x, double v2y)
	{
		double dot = v1x * v2x + v1y * v2y;
		double[] ret = new double[2];
		ret[0] = dot * v2x;
		ret[1] = dot * v2y;
		return ret;
	}
	
	private static double[] normalize(double px, double py)
	{
		double len = dist(0,0,px,py);
		double[] ret = new double[2];
		ret[0] = px/len;
		ret[1] = py/len;
		return ret;
	}
	
	private void computeEdgeCollision(Edge e1, Edge e2)
	{
		computeVertexEdgeCollision(e1.v1, ARROW_WIDTH, e2);
		computeVertexEdgeCollision(e1.v2, ARROW_WIDTH, e2);
		computeVertexEdgeCollision(e2.v1, ARROW_WIDTH, e1);
		computeVertexEdgeCollision(e2.v2, ARROW_WIDTH, e1);		
	}	
	
	private void enforceConstrains()
	{
		for (int i = 0; i < VERLET_ITERATIONS; i++)
		{
			// hard restrictions for vertices
			for (Iterator iter = vertices.iterator(); iter.hasNext();)
			{
				Vertex v = (Vertex) iter.next();
				if(v.px < 0.1)
				{
					v.px = 0.1;
					v.old_px = v.px;
				}
				if(v.px > 0.8)
				{
					v.px = 0.8;
					v.old_px = v.px;
				}
				if(v.py < 0.1)
				{
					v.py = 0.1;
					v.old_py = v.py;
				}
				if(v.py > 0.8)
				{
					v.py = 0.8;
					v.old_py = v.py;
				}
			}
			for (int j = 0; j < edges.size(); j++)
			{
				Edge e = (Edge) edges.elementAt(j);
				// ensure rest length
				double pushx = (e.v1.px - e.v2.px)*e.effort()*0.5;
				double pushy = (e.v1.py - e.v2.py)*e.effort()*0.5;
				e.v1.px += pushx;
				e.v1.py += pushy;
				e.v2.px -= pushx;
				e.v2.py -= pushy;
				// collisions against other segments
				for (int k = j+1; k < edges.size(); k++)
				{
					Edge e2 = (Edge) edges.elementAt(k);
					computeEdgeCollision(e,e2);
				}
			}
		}
	}
}
